
#ifndef PID_ANGLE_IR_SPEED_H
#define PID_ANGLE_IR_SPEED_H

#include "ti_msp_dl_config.h"


// PID控制器结构体定义
typedef struct {
    float Kp;           // 比例系数，用于快速响应当前误差，快速响应系统偏差
    float Ki;           // 积分系数，用于消除稳态误差，提高控制精度
    float Kd;           // 微分系数，用于预测误差变化趋势，抑制系统震荡
    float integral;     // 积分项累计值，用于累积历史误差
    float prevError;    // 上一次的误差值，用于计算微分项
    float outputLimit;  // 输出限制值，用于约束控制量的最大范围
    int lostCounter;    // 跟踪丢失计数器，记录目标丢失的次数
    float lastValidErr; // 最后一次有效的误差值，用于目标丢失时的过渡处理
} PIDController;

typedef struct 
{
    float SetValue;       // 目标设定值（期望达到的目标值）
    float GetValue;       // 反馈值（实际检测到的当前值）
    
    float T;              // PID控制周期（单位：秒）
    float Kp;             // 比例系数（Proportional gain）
    float Ti;             // 积分时间常数（Integral time）
    float Td;             // 微分时间常数（Derivative time）
    
    float EK;             // 当前偏差（当前周期的设定值与反馈值之差）
    float EK_1;           // 上一周期偏差（保存上一次计算的偏差）
    float SigmaEK;        // 偏差积分和（所有周期偏差的累积和）
        
    float Pout;           // 比例项输出（比例控制部分的计算结果）
    float Iout;           // 积分项输出（积分控制部分的计算结果）
    float Dout;           // 微分项输出（微分控制部分的计算结果）
    
    float Out0;           // 输出限幅基准值（用于限制输出范围的基准）
    float Out;            // PID总输出（比例+积分+微分的总和）
    
    unsigned int PIDCounter; // PID控制计数器（记录控制周期次数）
    unsigned int PWMCycle;   // PWM周期（用于将PID输出转换为PWM信号）
}PID_Struct;

void Motor_Angle_PID_Init_Left(void);
void Motor_Angle_PID_Cal_Left(void);
void Motor_Angle_PID_Init_Right(void);
void Motor_Angle_PID_Cal_Right(void);



// void PID_SpeedL_Init(PIDController *pid, float kp, float ki, float kd, float limit);
// float PID_SpeedL_Compute(PIDController *pid, float setpoint, float currentValue);

// void PID_SpeedR_Init(PIDController *pid, float kp, float ki, float kd, float limit);
// float PID_SpeedR_Compute(PIDController *pid, float setpoint, float currentValue);

void PID_Angle_Init(PIDController *pid, float kp, float ki, float kd, float limit);
float PID_Angle_Compute(PIDController *pid, float setpoint, float currentValue);

void PID_IR_Init(PIDController *pid, float kp, float ki, float kd, float limit);
float PID_IR_Compute(PIDController *pid, float setpoint, float currentValue);


#endif
